#pragma once

#include "cartographer/rcm/common.h"
#include "cartographer/sensor/imu_data.h"

/*
编译器自动生成默认拷贝构造函数和默认赋值运算符函数(均是浅拷贝，按字节拷贝)
https://blog.csdn.net/m0_69597277/article/details/132258115
https://blog.csdn.net/guoqx/article/details/121019998
*/

namespace cartographer
{
    const float GRAVITY_VALUE = 9.81;
    const Eigen::Vector3d GRAVITY_POSITIVE(0.0, 0.0, GRAVITY_VALUE);
    const Eigen::Vector3d GRAVITY_NEGATIVE(0.0, 0.0, -GRAVITY_VALUE);

    typedef std::vector<sensor::ImuData> ImuSet;
    typedef std::deque<sensor::ImuData> ImuDeque;

    struct ImuMeasure
    {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW ImuMeasure()
            : dt(0.0)
            , time(common::Time::min())
        {
        }

        ImuMeasure(const Eigen::Vector3d &acc, const Eigen::Vector3d &w, double dt_)
            : dt(dt_)
            , time(common::Time::min())
            , angular(w)
            , acceleration(acc)
        {
        }

        ImuMeasure(const Eigen::Vector3d &acc, const Eigen::Vector3d &w, const common::Time &tm)
            : dt(0.0)
            , time(tm)
            , angular(w)
            , acceleration(acc)
        {
        }

        ImuMeasure &operator=(const sensor::ImuData &imu)
        {
            time = imu.time;
            angular = imu.angular_velocity;
            acceleration = imu.linear_acceleration;
            return *this;
        }

        explicit ImuMeasure(const sensor::ImuData &imu)
            : dt(0.0)
            , time(imu.time)
            , angular(imu.angular_velocity)
            , acceleration(imu.linear_acceleration)
        {
        }

        double dt = 0.0;
        common::Time time;
        Eigen::Vector3d angular;
        Eigen::Vector3d acceleration;
    };

    typedef std::vector<ImuMeasure> ImuMeasSet;

    class PVR
    {
    public:
        PVR()
        {
        }
        Eigen::Vector3d P;
        Eigen::Matrix3d R;
        Eigen::Vector3d V;
        common::Time time;
    };

    class ImuBias
    {
    public:
        ImuBias()
        {
            // must set zero value
            acc_.setZero();
            gyro_.setZero();
        }

        ImuBias(const Eigen::Vector3d &acc, const Eigen::Vector3d &gyro)
            : acc_(acc)
            , gyro_(gyro)
        {
        }

        Eigen::Vector3d acc_;
        Eigen::Vector3d gyro_;
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

    inline bool HasNan(const ImuMeasure &data)
    {
        return HasNan(data.acceleration) || HasNan(data.angular);
    }

    inline bool HasNan(const sensor::ImuData &data)
    {
        return HasNan(data.linear_acceleration) || HasNan(data.angular_velocity);
    }

    inline void Print(const ImuMeasure &data)
    {
        LOG(INFO) << "imu " << common::ToUniversal(data.time) << " ang: " << data.angular(0) << "," << data.angular(1) << ","
                  << data.angular(2) << " acc: " << data.acceleration(0) << "," << data.acceleration(1) << "," << data.acceleration(2)
                  << "\n\n";
    }

    inline void Print(const sensor::ImuData &data)
    {
        LOG(INFO) << "imu " << common::ToUniversal(data.time) << " ang: " << data.angular_velocity(0) << "," << data.angular_velocity(1)
                  << "," << data.angular_velocity(2) << " acc: " << data.linear_acceleration(0) << "," << data.linear_acceleration(1) << ","
                  << data.linear_acceleration(2) << "\n\n";
    }

    inline void PrintPVR(const char *prefix, const PVR &pose)
    {
        const auto &rotation = QuatToEuler(Eigen::Quaterniond(pose.R));
        LOG(INFO) << "\n" << prefix << " trans:" << VecToStr(pose.P) << " rotat:" << VecToStr(rotation) << "\n";
    }

    inline Eigen::Matrix3d NormalizeRotation(const Eigen::Matrix3d &R)
    {
        Eigen::JacobiSVD<Eigen::Matrix3d> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV);
        return svd.matrixU() * svd.matrixV().transpose();
    }

    inline PVR AddPVR(const PVR &last, const PVR &incre, const Eigen::Vector3d &Gz, double dt)
    {
        PVR curr;
        curr.R = NormalizeRotation(last.R * incre.R);
        curr.P = last.P + last.V * dt + 0.5f * dt * dt * Gz + last.R * incre.P;
        curr.V = last.V + dt * Gz + last.R * incre.V;
        return curr;
    }

    int CollectImuByTimeSpan(const ImuDeque &imu_data, std::vector<sensor::ImuData> &imu_set, const common::Time &time_start,
                             const common::Time &time_end, const double timeOutThresh = 0.03);

    inline void ImuCompensateToStartTime(ImuMeasure &curr, const ImuMeasure &next, const common::Time &timeStart)
    {
        float diff = common::ToSeconds(curr.time - timeStart);
        float duration = common::ToSeconds(next.time - curr.time);
        float ratio = diff / duration;
        // 若当前IMU早于timeStart，则ratio为负数，因减法最终结果是速度增加，结果合理
        curr.angular = curr.angular - (next.angular - curr.angular) * ratio;
        curr.acceleration = curr.acceleration - (next.acceleration - curr.acceleration) * ratio;
        // LOG(INFO) << "midimu start " << diff << "/" << duration << "=" << ratio;
    }

    inline void ImuCompensateToEndTime(const ImuMeasure &curr, ImuMeasure &next, const common::Time &timeEnd)
    {
        float diff = common::ToSeconds(next.time - timeEnd);
        float duration = common::ToSeconds(next.time - curr.time);
        float ratio = diff / duration;
        next.angular = next.angular - (next.angular - curr.angular) * ratio;
        next.acceleration = next.acceleration - (next.acceleration - curr.acceleration) * ratio;
        // LOG(INFO) << "midimu end " << diff << "/" << duration << "=" << ratio;
    }

} // namespace cartographer